#include "logic_thread.hpp"

#include <math.h>
#include "stdlib.h"
#include "tim.h"

#include "UI.hpp"
#include "bsp_imu.hpp"
#include "chassis.hpp"
#include "dr16.hpp"
#include "gimbal.hpp"
#include "minipc.hpp"
#include "pid.h"
#include "referee.hpp"

#include "shoot.hpp"
#include "supercap.hpp"
#include "power_control.hpp"
#include <rtthread.h>
#include "flysky.hpp"

/* 选择遥控器数据来源 */
/* 选择遥控器数据来源 */
#if (USE_SBUS)
	FLYSKY		 flysky;
	#define RC flysky.get_fs_rc()
	#define PC vt.get_pc()
	#define FSRC flysky.get_fs_orin()
#else
	DR16     	 dr16;
	#define RC dr16.get_rc()
	#define PC dr16.get_pc()
#endif


VT_Link		 vt;
Power    power;
Chassis  chassis;
Gimbal   gimbal;
Shoot    shoot;
IMU      imu;
Supercap supercap;
UI       ui;

uint8_t   spinning_flag = 0;//小陀螺开启标志位
static bool  is_friction_launched;
static float time_count = 0;
bool  supercap_buffer_flag = false;
bool deploy_flag=false;//部署模式
uint8_t aimbot_state=0; //自瞄状态标志位
uint8_t supercap_state=0; //超电状态标志位
uint8_t *tem_spinning =& spinning_flag;
uint8_t *tem_aimbot =& aimbot_state;
uint8_t *tem_supercap=& supercap_state;
static float pitch_bias;
static float yaw_bias;
uint8_t fire=0;

int16_t counter_show=0;
static uint8_t Show_ui = 0;
extern uint8_t can_flag;
uint8_t counter_can=0;


void stop_command(void) {
    chassis.set_state(CHASSIS_STATE_STOP);
    gimbal.manual_mode(0, 0);
    shoot.set_friction(FRICTION_STATE_SLOW_OUT, FRICTION_MIN_SPEED,FRICTION_MIN_SPEED);
    shoot.set_plunk(PLUNK_STATE_STOP);
}

/**
 * @brief 底盘控制函数
 *
 */
void chassis_command() {
    float spinning_speed = 200;
    if (RC.left_sw_ == SW_DOWN) {  // PC 控制模式
      int16_t pc_speed_x = 0, pc_speed_y = 0;

			if(deploy_flag==false)
			{
				
				if (vt.is_key_pressed_down(KEY_SHIFT)) { 
				supercap_state =! supercap_state ;	
        }   
				if(supercap_state==1&&(power_heat_data.buffer_energy>40))
				{
            pc_speed_y = (vt.is_key_pressing(KEY_W) - vt.is_key_pressing(KEY_S)) *300;
            pc_speed_x = (vt.is_key_pressing(KEY_D) - vt.is_key_pressing(KEY_A)) *300;
				}
				else
				{
				    pc_speed_y = (vt.is_key_pressing(KEY_W) - vt.is_key_pressing(KEY_S)) *250;
            pc_speed_x = (vt.is_key_pressing(KEY_D) - vt.is_key_pressing(KEY_A)) *250;
				
				}
        

        if (spinning_flag == 1)
        {
						chassis.set_state(CHASSIS_STATE_SPINNING);//10 * (3 * sin(1.8 * 0.01 * time_count) + 14);
//					if(supercap_state==1)
//					{
//						spinning_speed=450;
//					
//					}
//					else if(game_robot_status.robot_level>=6)
//					{
//						spinning_speed = 350;
//					
//					}
//					else {spinning_speed = 200;}
//					
            chassis.spinning_mode(pc_speed_x, pc_speed_y, spinning_speed, gimbal.get_yaw(),
                                  YAW_ZERO_DEGREE);  // 小陀螺模式
        }
				else
        {
					  chassis.set_state(CHASSIS_STATE_MOVE);
            time_count = 0;
            chassis.follow_mode(pc_speed_x, pc_speed_y, gimbal.get_yaw(), YAW_ZERO_DEGREE);  // 正常模式
        }
			}else
			{
				chassis.set_state(CHASSIS_STATE_STOP);		
			}
			
    } else {
        /* 遥控器控制模式 */
        switch (RC.right_sw_) {
            case SW_UP:  // 小陀螺模式
//               chassis.set_state(CHASSIS_STATE_SPINNING);					supercap_state=1;
//               chassis.spinning_mode(RC.left_x_ - 1024, RC.left_y_ - 1024, spinning_speed, gimbal.get_yaw(),
//                                     YAW_ZERO_DEGREE);
						deploy_flag=1;power.dead_flag=0;can_flag=1;
                break;
            case SW_MID:  // 正常模式
                chassis.set_state(CHASSIS_STATE_MOVE);						supercap_state=0;deploy_flag=0;power.dead_flag=1;can_flag=1;
								
                chassis.follow_mode(RC.left_x_ - 1024, RC.left_y_ - 1024, gimbal.get_yaw(),YAW_ZERO_DEGREE);// gimbal.get_yaw(),YAW_ZERO_DEGREE
                break;
            case SW_DOWN:  // 停止模式
								chassis.set_state(CHASSIS_STATE_STOP);
                break;
        }
    }
}
/**
 * @brief 云台控制函数
 *
 */

first_order_filter_type_t mouse_x_filter, mouse_y_filter;
pid_type                  pitch_pid, yaw_pid;
fp32                      pitch_pid_param[5] = {60.0F, 0.00F, 40.0F, 100, 30};
fp32                      yaw_pid_param[5]   = {120.8F, 0.00F, 20.0F, 100, 30};
feedforward_type_t        pitch_ff, yaw_ff;
fp32                      pitch_yaw_ff_param = 50.0F;
void gimbal_command() {
		/* PC控制模式 */
    shoot.set_plunk(PLUNK_STATE_STOP);
   if (RC.left_sw_ == SW_DOWN)
	{
        minipc_recv_vison_behavior_t	temp_vison;
        static uint32_t        			last_tick;
        static bool            			need_clear = false;
		if(MiniPC_Receive_Vision(&temp_vison.shoot_angular_,&temp_vison.yaw_angular_) == MINIPC_ONLINE){
//	if (vt.is_button_pressing(BUTTON_RIGHT)&&MiniPC_Receive_Vision(&temp_vison.shoot_angular_,&temp_vison.yaw_angular_) == MINIPC_ONLINE)
// 	{
//			if (dr16.is_key_pressed_down(KEY_C)) {
//					pitch_bias -= 0.01F;
//			}
//			if (dr16.is_key_pressed_down(KEY_X)) {
//					pitch_bias += 0.01F;
//			}
//			if (dr16.is_key_pressed_down(KEY_V)) {
//					yaw_bias += 0.01F;
//			}
//			if (dr16.is_key_pressed_down(KEY_Z)) {
//					yaw_bias -= 0.01F;
//			}
//			if(dr16.is_key_pressed_down(KEY_B)){
//					yaw_bias=0;pitch_bias=0;
//			}
			temp_vison.shoot_angular_ += pitch_bias;
			temp_vison.yaw_angular_ += yaw_bias;
			need_clear = true;
			aimbot_state=1;
			float pitch_expected_speed = pid_calc(&pitch_pid, temp_vison.shoot_angular_, 0)-0;
 										//	feedforward_calc(&pitch_ff, temp_vison.shoot_angular_);
			float yaw_expected_speed   = pid_calc(&yaw_pid, temp_vison.yaw_angular_, 0)-
											feedforward_calc(&yaw_ff, temp_vison.yaw_angular_);
//	rt_kprintf("%d, %d, %d, %d\n", (int)temp_vison.yaw_angular_,(int) temp_vison.shoot_angular_,(int) yaw_expected_speed,(int)pitch_expected_speed);
			gimbal.manual_mode(pitch_expected_speed*100, yaw_expected_speed);						
			last_tick = rt_tick_get();
		} else {
            if (need_clear) {
                pid_clear(&pitch_pid);
                pid_clear(&yaw_pid);
                need_clear = false;
            }
						aimbot_state=0;
            first_order_filter_calc(&mouse_x_filter, PC.x_);
            first_order_filter_calc(&mouse_y_filter, PC.y_);
										
            gimbal.manual_mode(-mouse_y_filter.out*80, -mouse_x_filter.out*1.5);
        }

        if (vt.is_key_pressed_down(KEY_Q)) {
            gimbal.turn_around();
        }
        if (vt.is_key_pressed_down(KEY_F)) {
            spinning_flag = !spinning_flag;
        }
				if (vt.is_key_pressed_down(KEY_G)) {
            deploy_flag = !deploy_flag;
        }
    } else {
        /* 遥控器控制模式 */
        switch (RC.right_sw_) {
            case SW_UP:   //小陀螺模式
            case SW_MID:  //正常模式
                gimbal.manual_mode(-(RC.right_y_ - 1024)*6, (1024 - RC.right_x_) * 0.15);
                break;
            case SW_DOWN:  //停止模式
                break;
        }
    }
}

/**
 * @brief 发射机构控制函数
 *
 */
//4500 5900-16m/s  4000  5500
void shoot_command() {
    uint16_t   shoot_speed_limit = 6000;
    uint16_t   shoot_speed_limit_2 = 5000;
    /* 枪口热量限制 */
    static int last_shoot_speed;
    uint16_t allow_shoot = (game_robot_status.shooter_barrel_heat_limit 
                            - power_heat_data.shooter_id1_42mm_cooling_heat) / 100;
    static uint16_t shoot_num = 0;
    if (last_shoot_speed != shoot_data.initial_speed)
    {
        shoot_num++;
    }
		shoot.set_friction(FRICTION_STATE_SLOW_IN, shoot_speed_limit,shoot_speed_limit_2);
    if (RC.left_sw_ == SW_DOWN) {
        /* PC 控制模式 */

        /* 退弹*/
//        if(dr16.is_key_pressed_down(KEY_R)) {
//						shoot.tuidam();
//        }
        /* 启动摩擦轮 */
        if (vt.is_key_pressed_down(KEY_E)) {
            is_friction_launched = !is_friction_launched;
        }
        
        if (is_friction_launched) {
            /* 启动 */
           shoot.set_friction(FRICTION_STATE_SLOW_IN, shoot_speed_limit,shoot_speed_limit_2);
        } else {
            /* 停止 */ 
           shoot.set_friction(FRICTION_STATE_SLOW_OUT, FRICTION_MIN_SPEED,FRICTION_MIN_SPEED);
        }
        /* 连射 */
        if (vt.is_button_pressing(BUTTON_LEFT) && is_friction_launched
//					&&((int16_t)shoot_num < (int16_t)(allow_shoot - 1))
				) {
            shoot.set_plunk(PLUNK_STATE_WORK);
						shoot.shoot();
        } else {
            shoot.set_plunk(PLUNK_STATE_STOP);
            shoot_num = 0;
        }
    } else {
        switch (RC.left_sw_) {
            case SW_UP:  // 启动摩擦轮
                shoot.set_friction(FRICTION_STATE_SLOW_IN, shoot_speed_limit,shoot_speed_limit_2);
                if(RC.wheel_ >= 1600){
  //                  if ((int16_t)shoot_num < (int16_t)(allow_shoot - 1)){
									if(RC.right_sw_==SW_DOWN){
										if (fire==1){
                        shoot.set_plunk(PLUNK_STATE_WORK);
												shoot.shoot();
                    }
									}
									else{
                        shoot.set_plunk(PLUNK_STATE_WORK);
												shoot.shoot();
									}
                }
								else if(RC.wheel_ <=800){	shoot.set_plunk(PLUNK_STATE_WORK);shoot.tuidam();
								}
                else
                    shoot.set_plunk(PLUNK_STATE_STOP);
                break;
            case SW_MID:  // 停止摩擦轮
                shoot.set_friction(FRICTION_STATE_SLOW_OUT, FRICTION_MIN_SPEED,FRICTION_MIN_SPEED);
                shoot.set_plunk(PLUNK_STATE_STOP);
                break;
            case SW_DOWN:
                break;
        }
    }
}
extern uint8_t cap_err;
extern int16_t limit_power;
void ui_thread_entry(void *parameter) {
	
	while (1) {
		
	if(vt.is_key_pressed_down(KEY_R)) {
		counter_show=200;
	}
  
		if((counter_show--)>0)
		{
			ui.AddGraphic_all();
		}
		else{counter_show=0;}
		
		switch (Show_ui){
		case 0:			ui.friction_state(ModifyGraphic, shoot.get_friction_state());	break;
		case 1:			ui.BotLevel(ModifyGraphic, game_robot_status.robot_level);break;	//bullet_remaining.projectile_allowance_17mm);	break;		
		case 2:			ui.aimbot_state(ModifyGraphic, *tem_aimbot);	break;		
		case 3:			ui.ChassisFace(ModifyGraphic,gimbal.get_yaw(),30);	break;		
		case 4:			ui.deploy_state(ModifyGraphic, deploy_flag);	break;	
		case 5:			ui.CapVoltage(ModifyGraphic,power.V_SUPERCAP);	break;		
		case 6:			ui.supercap_state(ModifyGraphic, *tem_supercap);	break;	
		//case 7:     ui.Rectangle(AddGraphic,1750,1800,500,800);
	}
	Show_ui++;
	Show_ui %=8;
	
	rt_thread_mdelay(5);			
	}

}
/**
 * @brief   主逻辑线程
 *
 * @param   parameter //入口参数
 *
 */
void logic_thread_entry(void *parameter) {
    //初始化
    // 等待 IMU 初始化
    #if (USE_SBUS)
			flysky.init();
    #else
			dr16.init();  
	  #endif
	  imu.init();
    referee_init();

		rt_thread_mdelay(500);
		Minipc_init();
    gimbal.init();
    shoot.init();
		power.init();
    chassis.init();

    /* 一阶低通滤波初始化 */
    first_order_filter_init(&mouse_x_filter, 0.01, 0.4);
    first_order_filter_init(&mouse_y_filter, 0.01, 0.4);
    // Minipc控制云台pid初始化
    pid_init(&pitch_pid, PID_POSITION, pitch_pid_param);
    pid_init(&yaw_pid, PID_POSITION, yaw_pid_param);
    /* 前馈控制初始化 */
    feedforward_init(&pitch_ff, pitch_yaw_ff_param);
    feedforward_init(&yaw_ff, pitch_yaw_ff_param);


    while (1) {
        if (RC.right_sw_ == SW_DOWN) {
					stop_command();     
       }
        else {
            gimbal_command();
						chassis_command();
            shoot_command();
						

            rt_thread_mdelay(5);			
        }
        
//        /* 变速小陀螺时间记录 */
//        if (spinning_flag == 1) {
//            time_count+=1;
//        }
//        else {
//            time_count = 0;
//        }
  }
}
